import rosbag
from colorama import Fore, Style
import os
import sys
import warnings
import def_function

# 获取当前脚本的完整路径
script_path = os.path.abspath(sys.argv[0])
# 从完整路径中获取目录
script_dir = os.path.dirname(script_path)
# 从完整路径中分离出文件名
script_name = os.path.basename(script_path)
# 使用 splitext() 函数分离文件名和扩展名
script_name_without_extension, extension = os.path.splitext(script_name)

print(Fore.YELLOW+f"python script name:", sys.argv[0])
if os.getenv('DEBUG') == 'true':
    python_param = '/mnt/01_code/python/rosbag_analysis/dataspace/ROCKY/PL077159_recording_dfdi_console_manual_recording_20230904-104320_20230904-104346_0_filter.bag'
else:
    if len(sys.argv)!=2:
        print(Fore.RED+f"you need python params!")
        sys.exit()
    python_param = script_dir + "/" + sys.argv[1]
    if os.path.isabs(sys.argv[1]):
        python_param = sys.argv[1]
rosbag_data = rosbag.Bag(python_param)

# 获取类型和主题信息
# rosbag_info = rosbag_data.get_type_and_topic_info()
# info.topics 是一个字典，它的键是 topic 的名字
# all_topics = list(rosbag_info.topics.keys())
#topics = ['/vehicle/chassis_report', '/vehicle/wheel_report', '/vehicle/body_report', '/mla/egopose', '/mb/sensor/imu']
topics = ['/vehicle/chassis_report', '/vehicle/wheel_report', '/vehicle/body_report']
error_msg_arry = []
dict_msg = dict()
dict_error_info = dict()

class diff_info:
    def __init__(self):
        self.seq = []
        self.header = []
        self.meta = []
dict_dff_info = dict()
dict_judge = dict()

cnt = 0
for msg_topic, msg, t in rosbag_data.read_messages(topics=topics):
    if dict_msg.get(msg_topic) is not None:
        #计算差值
        if dict_dff_info.get(msg_topic) is None:
            dict_dff_info.setdefault(msg_topic, diff_info())
        dff = dict_dff_info[msg_topic]
        dff.seq.append(msg.header.seq - dict_msg[msg_topic].header.seq)
        dff.header.append((msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) - (dict_msg[msg_topic].header.stamp.secs * 1000000 + dict_msg[msg_topic].header.stamp.nsecs / 1000))
        dff.meta.append(msg.meta.timestamp_us - dict_msg[msg_topic].meta.timestamp_us)

    error_type = "当前帧header时间戳跳0"
    if dict_error_info.get(error_type+msg_topic) is None:
        dict_error_info.setdefault(error_type+msg_topic, def_function.class_data.error_info())
    err_info = dict_error_info[error_type+msg_topic]
    judge_condition = "当前帧header时间戳跳等于0"
    if msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000 == 0:
        if  err_info.start_time == 0.0:
            err_info.start_time = t.to_sec()
            err_info.message = "[" + error_type + "{" + judge_condition + "}]" + msg_topic
    else:
        err_info.end_time = t.to_sec()
        if err_info.start_time != 0.0:
            err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
            error_msg_arry.append(err_info.message)
            err_info.start_time = 0.0
    
    error_type = "当前帧meta时间戳跳0"
    if dict_error_info.get(error_type+msg_topic) is None:
        dict_error_info.setdefault(error_type+msg_topic, def_function.class_data.error_info())
    err_info = dict_error_info[error_type+msg_topic]
    judge_condition = "当前帧meta时间戳跳等于0"
    if msg.meta.timestamp_us == 0:
        if  err_info.start_time == 0.0:
            err_info.start_time = t.to_sec()
            err_info.message = "[" + error_type + "{" + judge_condition + "}]" + msg_topic
    else:
        err_info.end_time = t.to_sec()
        if err_info.start_time != 0.0:
            err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
            error_msg_arry.append(err_info.message)
            err_info.start_time = 0.0
    
    error_type = "当前帧header减meta时间戳不为正数"
    if dict_error_info.get(error_type+msg_topic) is None:
        dict_error_info.setdefault(error_type+msg_topic, def_function.class_data.error_info())
    err_info = dict_error_info[error_type+msg_topic]
    judge_condition = "当前帧header减meta时间戳小于或等于0"
    if (msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) - msg.meta.timestamp_us <= 0 :
        if  err_info.start_time == 0.0:
            err_info.start_time = t.to_sec()
            err_info.message = "[" + error_type + "{" + judge_condition + "}]" + msg_topic + " header:" + str(msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) + " meta:" + str(msg.meta.timestamp_us) + " 差值:" + str((msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) - msg.meta.timestamp_us)
    else:
        err_info.end_time = t.to_sec()
        if err_info.start_time != 0.0:
            err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
            error_msg_arry.append(err_info.message)
            err_info.start_time = 0.0
    
    error_type = "当前帧header减meta时间戳超规定范围"
    if dict_error_info.get(error_type+msg_topic) is None:
        dict_error_info.setdefault(error_type+msg_topic, def_function.class_data.error_info())
    err_info = dict_error_info[error_type+msg_topic]
    judge_condition = "当前帧header减meta时间戳大于120ms"
    if (msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) - msg.meta.timestamp_us > 120000 :
        if  err_info.start_time == 0.0:
            err_info.start_time = t.to_sec()
            err_info.message = "[" + error_type + "{" + judge_condition + "}]" + msg_topic + " header:" + str(msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) + " meta:" + str(msg.meta.timestamp_us) + " 差值:" + str((msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) - msg.meta.timestamp_us)
    else:
        err_info.end_time = t.to_sec()
        if err_info.start_time != 0.0:
            err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
            error_msg_arry.append(err_info.message)
            err_info.start_time = 0.0
    
    for key, value in dict_dff_info.items():
        error_type = "前后帧序列不累计"
        if dict_error_info.get(error_type+key) is None:
            dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
        err_info = dict_error_info[error_type+key]
        judge_condition = "前后帧序列差值等于0"
        if value.seq[len(value.seq)-1] == 0 :
            if  err_info.start_time == 0.0:
                err_info.start_time = t.to_sec()
                err_info.message = "[" + error_type + "{" + judge_condition + "}]" + key + " 上一帧:" + str(dict_msg[key].header.seq) + " 当前帧:" + str(msg.header.seq) + " 差值:" + str(value.seq[len(value.seq)-1])
        else:
            err_info.end_time = t.to_sec()
            if err_info.start_time != 0.0:
                err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
                error_msg_arry.append(err_info.message)
                err_info.start_time = 0.0

        error_type = "前后帧序列不连续"
        if dict_error_info.get(error_type+key) is None:
            dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
        err_info = dict_error_info[error_type+key]
        judge_condition = "前后帧序列差值大于1"
        if value.seq[len(value.seq)-1] > 1 :
            if  err_info.start_time == 0.0:
                err_info.start_time = t.to_sec()
                err_info.message = "[" + error_type + "{" + judge_condition + "}]" +  key + " 上一帧:" + str(dict_msg[key].header.seq) + " 当前帧:" + str(msg.header.seq) + " 差值:" + str(value.seq[len(value.seq)-1])
        else:
            err_info.end_time = t.to_sec()
            if err_info.start_time != 0.0:
                err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
                error_msg_arry.append(err_info.message)
                err_info.start_time = 0.0
        
        # https://momenta.feishu.cn/wiki/wikcnIiyebT5gsi6tdduby3f7Tc#VIAedqUYUo8EYWxS62GcPGKHnIf
        error_type = "前后帧header时间轴回退"
        if dict_error_info.get(error_type+key) is None:
            dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
        err_info = dict_error_info[error_type+key]
        judge_condition = "前后帧header差值大于小于或等于0"
        if value.header[len(value.header)-1] <= 0 :
            if  err_info.start_time == 0.0:
                err_info.start_time = t.to_sec()
                err_info.message = "[" + error_type + "{" + judge_condition + "}]" +  key + " 上一帧:" + str(dict_msg[key].header.stamp.secs * 1000000 + dict_msg[key].header.stamp.nsecs / 1000) + " 当前帧:" + str(msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) + " 差值:" + str(value.header[len(value.header)-1])
        else:
            err_info.end_time = t.to_sec()
            if err_info.start_time != 0.0:
                err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
                error_msg_arry.append(err_info.message)
                err_info.start_time = 0.0
        error_type = "前后帧meta时间轴回退"
        if dict_error_info.get(error_type+key) is None:
            dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
        err_info = dict_error_info[error_type+key]
        judge_condition = "前后帧meta差值大于小于0"
        if value.meta[len(value.meta)-1] < 0 :
            if  err_info.start_time == 0.0:
                err_info.start_time = t.to_sec()
                err_info.message = "[" + error_type + "{" + judge_condition + "}]" +  key + " 上一帧:" + str(dict_msg[key].meta.timestamp_us) + " 当前帧:" + str(msg.meta.timestamp_us) + " 差值:" + str(value.meta[len(value.meta)-1])
        else:
            err_info.end_time = t.to_sec()
            if err_info.start_time != 0.0:
                err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
                error_msg_arry.append(err_info.message)
                err_info.start_time = 0.0

        keep_time = 0.20
        keep_cnt = 10
        error_type = "前后帧meta时间不变"
        if dict_judge.get(error_type+key) is None:
            dict_judge.setdefault(error_type+key+"时间", def_function.judge_value.ValueJudge())
            dict_judge.setdefault(error_type+key+"次数", def_function.judge_value.ValueJudge())
        if dict_error_info.get(error_type+key) is None:
            dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
        err_info = dict_error_info[error_type+key]
        result = dict_judge[error_type+key+"时间"].timeout(t.to_sec(), value.meta[len(value.meta)-1], 0, keep_time, "=="); 
        result1 = dict_judge[error_type+key+"次数"].cntout(t.to_sec(), value.meta[len(value.meta)-1], 0, keep_cnt, "=="); 
        judge_condition = "前后帧meta时间差值等于0并持续一段时间或次数(" + str(keep_time) + "秒/"+ str(keep_cnt) +"次)"
        if result or result1:
            if  err_info.start_time == 0.0:
                err_info.start_time = t.to_sec()
                err_info.message = "[" + error_type + "{" + judge_condition + "}]" +  key + " 上一帧:" + str(dict_msg[key].meta.timestamp_us) + " 当前帧:" + str(msg.meta.timestamp_us) + " 差值:" + str(value.meta[len(value.meta)-1])
        else:
            err_info.end_time = t.to_sec()
            if err_info.start_time != 0.0:
                err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
                error_msg_arry.append(err_info.message)
                err_info.start_time = 0.0

        average = sum(value.header) / len(value.header)
        factor = 0.33
        if "/mb/sensor/" in key:
            factor = 0.1
        error_type = "前后帧header性能不稳定波动厉害"
        if dict_error_info.get(error_type+key) is None:
            dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
        err_info = dict_error_info[error_type+key]
        judge_condition = "前后帧header间隔小于" + str(factor) + "*平均值"
        if value.header[len(value.header)-1] < average * factor and value.header[len(value.header)-1] > 0:
            if  err_info.start_time == 0.0:
                err_info.start_time = t.to_sec()
                err_info.message = "[" + error_type + "{" + judge_condition + "}]" +  key + " 上一帧:" + str(dict_msg[key].header.stamp.secs * 1000000 + dict_msg[key].header.stamp.nsecs / 1000) + " 当前帧:" + str(msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) + " 差值:" + str(value.header[len(value.header)-1]) + " 平均值:" + str(average) + "-" + str(average * factor)
        else:
            err_info.end_time = t.to_sec()
            if err_info.start_time != 0.0:
                err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
                error_msg_arry.append(err_info.message)
                err_info.start_time = 0.0
        factor = 3
        if "/mb/sensor/" in key:
            factor = 10
        error_type = "前后帧header丢帧或系统卡顿或时间轴往前跳跃了"
        if dict_error_info.get(error_type+key) is None:
            dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
        err_info = dict_error_info[error_type+key]
        judge_condition = "前后帧header间隔大于" + str(factor) + "*平均值"
        if value.header[len(value.header)-1] > average * factor :
            if  err_info.start_time == 0.0:
                err_info.start_time = t.to_sec()
                err_info.message = "[" + error_type + "{" + judge_condition + "}]" +  key + " 上一帧:" + str(dict_msg[key].header.stamp.secs * 1000000 + dict_msg[key].header.stamp.nsecs / 1000) + " 当前帧:" + str(msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) + " 差值:" + str(value.header[len(value.header)-1]) + " 平均值:" + str(average) + "-" + str(average * factor)
        else:
            err_info.end_time = t.to_sec()
            if err_info.start_time != 0.0:
                err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
                error_msg_arry.append(err_info.message)
                err_info.start_time = 0.0

        average = sum(value.meta) / len(value.meta)
        factor = 0.33
        if "/mb/sensor/" in key:
            factor = 0.1
        error_type = "前后帧meta性能不稳定波动厉害"
        if dict_error_info.get(error_type+key) is None:
            dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
        err_info = dict_error_info[error_type+key]
        judge_condition = "前后帧meta间隔小于" + str(factor) + "*平均值"
        if value.meta[len(value.meta)-1] < average * factor and value.meta[len(value.meta)-1] > 0:
            if  err_info.start_time == 0.0:
                err_info.start_time = t.to_sec()
                err_info.message = "[" + error_type + "{" + judge_condition + "}]" +  key + " 上一帧:" + str(dict_msg[key].meta.timestamp_us) + " 当前帧:" + str(msg.meta.timestamp_us) + " 差值:" + str(value.meta[len(value.meta)-1]) + " 平均值:" + str(average) + "-" + str(average * factor)
        else:
            err_info.end_time = t.to_sec()
            if err_info.start_time != 0.0:
                err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
                error_msg_arry.append(err_info.message)
                err_info.start_time = 0.0

        factor = 3
        if "/mb/sensor/" in key:
            factor = 10
        error_type = "前后帧meta丢帧或系统卡顿或时间轴往前跳跃了"
        if dict_error_info.get(error_type+key) is None:
            dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
        err_info = dict_error_info[error_type+key]
        judge_condition = "前后帧meta间隔大于" + str(factor) + "*平均值"
        if value.meta[len(value.meta)-1] > average * factor:
            if  err_info.start_time == 0.0:
                err_info.start_time = t.to_sec()
                err_info.message = "[" + error_type + "{" + judge_condition + "}]" +  key + " 上一帧:" + str(dict_msg[key].meta.timestamp_us) + " 当前帧:" + str(msg.meta.timestamp_us) + " 差值:" + str(value.meta[len(value.meta)-1]) + " 平均值:" + str(average) + "-" + str(average * factor)
        else:
            err_info.end_time = t.to_sec()
            if err_info.start_time != 0.0:
                err_info.message = "[" + str(err_info.start_time) + "-" + str(err_info.end_time) + "]" + err_info.message
                error_msg_arry.append(err_info.message)
                err_info.start_time = 0.0

    dict_msg[msg_topic] = msg
################################################################################################
rosbag_data.close()
for value in dict_error_info.values():
    if value.start_time != 0.0:
        error_msg_arry.append("[" + str(value.start_time) + "-bag_end]" + value.message)

if error_msg_arry:
    print(Fore.RED+f"", script_name_without_extension)
    # 打印数组中的每个元素, 每个元素一行
    for item in error_msg_arry:
        print(Fore.CYAN+f"", item)

